//
// Created by s on 2025/10/13.
//

#include "kinodynamic.h"

const float DIAMETER = 22.92f; /*电机输出轴同步轮直径*/
const float SPEED = 100.0f; /*实际工具头运行速度，单位是：mm/s*/
const float SCALE = 0.35f; /*电机单测加减速占比系数*/

MotorCfg_Down PlanMotorMove(MotorCfg_Up *cfg_up, float tool_distance)
{
    //重新进行运动学规划，考虑到电机加减速
    /*确定方向*/
    const uint8_t dir_a = cfg_up->direction_a;
    const uint8_t dir_b = cfg_up->direction_b;

    /*确定单次动作运行总时间,单位是分*/
    const float TIME = 60 * 2 * tool_distance / SPEED;

    /*确定电机运行路程，单位是转*/
    const float round_a = cfg_up->distance_a / ((float) M_PI * DIAMETER);
    const float round_b = cfg_up->distance_b / ((float) M_PI * DIAMETER);


    /*确定电机运行速度，单位是rpm*/
    const float vel_a = round_a / (TIME - TIME * SCALE);
    const float vel_b = round_a / (TIME - TIME * SCALE);

    /*确定电机加速度，单位是转每二次方秒*/
    const float acc_a = 256.0f - ((6000000 * round_a * TIME * SCALE) / (5 * (TIME - 1)));
    const float acc_b = 256.0f - ((6000000 * round_b * TIME * SCALE) / (5 * (TIME - 1)));


    const MotorCfg_Down motor_cfg_down = {
        .direction_a = dir_a, .velocity_a = vel_a, .angle_a = round_a, .acceleration_a = acc_a, .direction_b = dir_b, .velocity_b = vel_b,
        .angle_b = round_b, .acceleration_b = acc_b
    };

    return motor_cfg_down;

    // /*确定旋转方向*/
    // MotorCfg_Down cfg_down = {
    //     .direction_a = cfg_up.direction_a, .direction_b = cfg_up.direction_b,
    // };
    //
    // /*确定旋转速度*/
    // const float TIME = final_distance / 100.0f;
    // float vel_a = (cfg_up.distance_a * 100.0f) / TIME;
    // float vel_b = (cfg_up.distance_b * 100.0f) / TIME;
    // cfg_down.velocity_a = vel_a;
    // cfg_down.velocity_b = vel_b;
    //
    // /*确定旋转角度*/
    // float angle_a = 360.0f / (DIAMETER * M_PI) * cfg_up.distance_a;
    // float angle_b = 360.0f / (DIAMETER * M_PI) * cfg_up.distance_b;
    // cfg_down.angle_a = angle_a;
    // cfg_down.angle_b = angle_b;
    //
    // return cfg_down;
}

float cordi_2_distance(Coordinate cordi_now, Coordinate cordi_target)
{
    float dx = cordi_now.x_value - cordi_target.x_value;
    float dy = cordi_now.y_value - cordi_target.y_value;
    return sqrtf(dx * dx + dy * dy);
}
